/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "zeromanagement.h"

#define D_PARAMETER_NODE_COUNT 18

ZeroManagement::ZeroManagement(QString fileNameIn, int robotTypeIn, int robotIdIn, int jointNumIn,
                               MotionMessage *motionMessageIn, JointDecouple* jointDecoupleIn)
{
    fileName=fileNameIn;
    robotId=robotIdIn;
    robotType=robotTypeIn;
    jointNum=jointNumIn;
    motionMessage=motionMessageIn;
    jointDecouple=jointDecoupleIn;

    absoluteZeroMotorValue.resize(jointNum);
    jointMechanicalZeroValue.resize(jointNum);
    axisZeroState.resize(jointNum);
    isAbsoluteEncoder.resize(jointNum);
    zeroReturnParameterList.resize(jointNum);
    for(int i=0;i<jointNum;i++)
    {
        axisZeroState[i]=ENUM_AXIS_ZERO_STATE_NO_CALIBRATE;
        absoluteZeroMotorValue[i]=0;
        jointMechanicalZeroValue[i]=0;
    }
    status=ENUM_ZERO_MANAGEMENT_STATE_OK;
    readFile();

    #ifdef D_USE_MD5_CHECK
    checkFile();//md5校验
    #endif

    checkIncrementEncoder();

    validateZeroData();

    zeroMovingType=EM_ZERO_MOVINT_TYPE_NONE;
    isHalt=true;
    currentZeroMovingAxisId=-1;

}

E_ZERO_TASK_MODE ZeroManagement::getZeroTaskMode()
{
    return zeroTaskMode;
}

int ZeroManagement::calibrateJointByAiEncoder(int axisId, double aiValue)
{
    QVector<MotorData> tmpMotorData=jointDecouple->getMotorDataFeedbackVector();
    //当前正确的编码器值
    int tmpCorrentData=(aiValue-zeroReturnParameterList[axisId].aiEncoderOffset)*
            zeroReturnParameterList[axisId].aiEncoderRatio;
    //DH零点时编码器的值
    absoluteZeroMotorValue[axisId]=tmpMotorData[axisId].position-tmpCorrentData;


    validateZeroData();
    axisZeroState[axisId]=ENUM_AXIS_ZERO_STATE_CALIBRATED;
    qDebug()<<"calibrateJointByAiEncoder axisId"<<axisId<<"aiValue"<<aiValue<<"tmpCorrentData"<<tmpCorrentData
              <<"feedbackPosition"<<tmpMotorData[axisId].position<<"absoluteZeroMotorValue"<<absoluteZeroMotorValue[axisId];
    return 1;
}

bool ZeroManagement::isAbsoulteMotor(int axisId)
{
    return isAbsoluteEncoder[axisId];

}

int ZeroManagement::getZeroMovingType(EM_ZERO_MOVINT_TYPE &zeroMovingTypeOut)
{
    zeroMovingTypeOut=zeroMovingType;
    return 1;
}

int ZeroManagement::startGoZero(int axisId)
{
    qDebug()<<"ZeroManagement::startGoZero axisId"<<axisId;
    zeroMovingType=EM_ZERO_MOVINT_TYPE_ONE_AXIS;
    currentZeroMovingAxisId=axisId;
    isHalt=false;
    return 1;
}

int ZeroManagement::startGoZero()
{
    qDebug()<<"ZeroManagement::startGoZero all axis";
    zeroMovingType=EM_ZERO_MOVINT_TYPE_ALL_AXISES;
    axisIdtoGoZero.clear();

    QVector<int> tmpSequence;
    tmpSequence.resize(jointNum);
    for(int i=0;i<jointNum;i++)
    {
        tmpSequence[i]=zeroReturnParameterList[i].goZeroSequence;
    }
    qSort(tmpSequence.begin(),tmpSequence.end());
    for(int sequence=0;sequence<jointNum;sequence++)
    {
        for(int i=0;i<jointNum;i++)
        {
            if(tmpSequence[sequence]==zeroReturnParameterList[i].goZeroSequence
                    && 1==zeroReturnParameterList[i].isGoZero)
            {
                axisIdtoGoZero.append(i);
                qDebug()<<"add go zero axisId="<<i;
            }
        }

    }
    if(axisIdtoGoZero.size()>0)
    {
        currentZeroMovingAxisId=axisIdtoGoZero[0];
        qDebug()<<"currentZeroMovingAxisId="<<currentZeroMovingAxisId;
    }
    else
    {
        currentZeroMovingAxisId=-1;
        qDebug()<<"axisIdtoGoZero.size()<1,currentZeroMovingAxisId=-1";
    }
    isHalt=false;
    return 1;
}

int ZeroManagement::haltGoZero()
{
    isHalt=true;
    return 1;
}

int ZeroManagement::getGoZeroAxis(int &axisId)
{
    if(currentZeroMovingAxisId>=jointNum  || currentZeroMovingAxisId<0)
    {
        qDebug()<<"error,currentZeroMovingAxisId>=jointNum  || currentZeroMovingAxisId<0";
        return -1;
    }

    if(false==isHalt)
    {
        if(EM_ZERO_MOVINT_TYPE_ONE_AXIS==zeroMovingType)
        {

            axisId=currentZeroMovingAxisId;

            return 1;
        }
        else if(EM_ZERO_MOVINT_TYPE_ALL_AXISES==zeroMovingType)
        {
            axisId=currentZeroMovingAxisId;
            return 1;
        }
        else
        {
            qDebug()<<"unknow zeroMovingType"<<zeroMovingType;
            return -1;
        }

    }
    qDebug()<<"error,true==isHalt";
    return -1;
}

int ZeroManagement::changeToNextGoZeroAxis()
{
    qDebug()<<"ZeroManagement::changeToNextGoZeroAxis";
    if(axisIdtoGoZero.size()>0)
    {
        axisIdtoGoZero.removeFirst();
    }
    else
    {
        return -1;
    }
    if(axisIdtoGoZero.size()>0)
    {
        currentZeroMovingAxisId=axisIdtoGoZero[0];
        return 1;
    }
    currentZeroMovingAxisId=-1;
    return -1;
}

int ZeroManagement::loopRealTime()
{

}

int ZeroManagement::loopCommonTime()
{

}

int ZeroManagement::getCalibrateInformation(S_AXISGROUP_ZERO_INFORMATION &zeroInformationOut)
{
    zeroInformationOut.state=status;
    zeroInformationOut.axisZeroState=axisZeroState.toStdVector();
    zeroInformationOut.absoluteZeroMotorValue=absoluteZeroMotorValue.toStdVector();
    zeroInformationOut.jointMechanicalZeroValue=jointMechanicalZeroValue.toStdVector();
    return 1;
}

int ZeroManagement::checkFile()
{
    MD5Check md5Checker;
    if(-1==md5Checker.isMd5CheckOk(fileName,fileName+".md5"))
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::checkFile() error！";
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_MD5_ERROR;
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "ZeroManagement", 5406, robotId);

        return -1;
    }
}

int ZeroManagement::createMd5File()
{
    MD5Check md5Checker;
    md5Checker.createMd5File(fileName);
    return 1;

}

bool ZeroManagement::isAxisZeroOk()
{
    for(int i=0;i<jointNum;i++)
    {
        if(false==isAxisZeroOk(i))
        {
            return false;
        }
    }

    if(status==ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR)
    {
        return false;
    }

    return true;

}

bool ZeroManagement::isOnlyAxisZeroOk()
{
    for(int i=0;i<jointNum;i++)
    {
        if(false==isAxisZeroOk(i))
        {
            return false;
        }
    }
    return true;

}

bool ZeroManagement::isAxisZeroOk(int axisId)
{

    if(ENUM_AXIS_ZERO_STATE_CALIBRATED!=axisZeroState[axisId])
    {
        if(false==isAbsoluteEncoder[axisId] && 0==zeroReturnParameterList[axisId].isNeedGoZeroAfterBoot)
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    return true;


}

int ZeroManagement::getAxisZeroState(int axisId)
{
    return axisZeroState[axisId];
}

QVector<int> ZeroManagement::getAbsoluteZeroMotorValue()
{
    return absoluteZeroMotorValue;
}

QVector<double> ZeroManagement::getJointMechanicalZeroValue()
{
    return jointMechanicalZeroValue;
}

int ZeroManagement::getZeroReturnParameter(QVector<ZeroReturnParameter> &parameterOut)
{
    parameterOut=zeroReturnParameterList;
    return 1;
}

int ZeroManagement::axisLosePower()
{
    for(int i=0;i<jointNum;i++)
    {
        axisZeroState[i]=ENUM_AXIS_ZERO_STATE_LOSS_POWER;
    }
    if(1==writeFile())
    {
        MD5Check md5Checker;
        md5Checker.createMd5File(fileName);
        status=ENUM_ZERO_MANAGEMENT_STATE_OK;
        readFile();
        checkFile();
        return 1;
    }
    else
    {
        return -1;
    }
    return 1;
}

int ZeroManagement::axisLosePower(int axisId)
{
    if(axisId>=jointNum || axisId<0)
    {
        qDebug()<<"axisLosePower error, jointValue.size()!=jointNum";
        return -1;
    }
    axisZeroState[axisId]=ENUM_AXIS_ZERO_STATE_LOSS_POWER;
    if(1==writeFile())
    {
        MD5Check md5Checker;
        md5Checker.createMd5File(fileName);
        status=ENUM_ZERO_MANAGEMENT_STATE_OK;
        readFile();
        checkFile();
        return 1;
    }
    else
    {
        return -1;
    }
    return 1;
}

int ZeroManagement::calibrateRobotByOffset(std::vector<double> offsetIn)
{
    qDebug()<<"ZeroManagement::calibrateRobotByOffset";
    //忽略轴之间耦合影响。
    for(int i=0;i<offsetIn.size();i++)
    {
        absoluteZeroMotorValue[i]-=jointDecouple->translateJointToPulse(i,offsetIn[i]);
        qDebug()<<"offsetIn[i]"<<offsetIn[i]<<"absoluteZeroMotorValue[i]"<<absoluteZeroMotorValue[i];
    }
    if(1==writeFile())
    {
        if(true==isOnlyAxisZeroOk())
        {
            MD5Check md5Checker;
            md5Checker.createMd5File(fileName);
            status=ENUM_ZERO_MANAGEMENT_STATE_OK;
            readFile();
            checkFile();
        }

       return 1;
    }
    else
    {
        return -1;
    }
}

int ZeroManagement::validateZeroData()
{
    jointDecouple->setAbsoluteZeroMotorValue(absoluteZeroMotorValue);
    jointDecouple->setJointMechanicalZeroValue(jointMechanicalZeroValue);
    return 1;
}

int ZeroManagement::calibrateRobot()
{
    jointDecouple->recordCurrentPosition2AbsoluteZeroMotorValue(absoluteZeroMotorValue);
    jointDecouple->setJointMechanicalZeroValue(jointMechanicalZeroValue);
    for(int i=0;i<jointNum;i++)
    {
        axisZeroState[i]=ENUM_AXIS_ZERO_STATE_CALIBRATED;
    }
    if(1==writeFile())
    {
        MD5Check md5Checker;
        md5Checker.createMd5File(fileName);
        status=ENUM_ZERO_MANAGEMENT_STATE_OK;
        readFile();
        checkFile();
        return 1;
    }
    else
    {
        return -1;
    }


}

int ZeroManagement::calibrateRobot(int axisId)
{
    if(axisId>=jointNum || axisId<0)
    {
        qDebug()<<"calibrateRobot error, jointValue.size()!=jointNum";
        return -1;
    }
    jointDecouple->recordCurrentPosition2AbsoluteZeroMotorValue(absoluteZeroMotorValue[axisId],axisId);
    jointDecouple->setJointMechanicalZeroValue(jointMechanicalZeroValue);
    axisZeroState[axisId]=ENUM_AXIS_ZERO_STATE_CALIBRATED;

    if(1==writeFile())
    {
        if(true==isOnlyAxisZeroOk())
        {
            MD5Check md5Checker;
            md5Checker.createMd5File(fileName);
            status=ENUM_ZERO_MANAGEMENT_STATE_OK;
            readFile();
            checkFile();
        }

       return 1;
    }
    else
    {
        return -1;
    }
}

int ZeroManagement::readFile()
{
    bool ok = openXml(fileName, QIODevice::ReadOnly );
    if( !ok )//无法打开机器人数据文件！
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====无法打开机器人零点配置文件！";
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "ZeroManagement", 5400, robotId);

        return -1;
    }
    int robotAxisesCount=0;

    QDomNode  tmpNode;

//    //读机器人类型
//    int robot;
//    QDomNode  typeNode = findSubNode(m_root, "commonSetting", ok );
//    if( !ok )
//    {
//        if(IF_DEBUG)
//        {
//            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<fileName;
//        }
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5201, robotId);
//        return -1;
//    }

//    readXml( findSubNode(typeNode, "robot", ok), robot  );
//    if( !ok)
//    {
//        if(IF_DEBUG)
//        {
//            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotType";
//        }
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId);
//        return -1;
//    }


    ///读参数
    QDomNode  inNode = findSubNode(m_root, "robotAxis", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====读文件失败: robotAxis"<<fileName;
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5401,robotId);
        return -1;
    }

    readXml( findSubNode(inNode, "count", ok), robotAxisesCount  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====读文件失败: robotAxis count"<<robotAxisesCount<<fileName;
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5402,robotId);
        return -1;
    }

    int tmpmode;
    readXml( findSubNode(inNode, "zeroTaskMode", ok),  tmpmode );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====读文件失败: zeroTaskMode"<<zeroTaskMode<<fileName;
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5409,robotId);
        return -1;
    }
    zeroTaskMode=(E_ZERO_TASK_MODE)tmpmode;

    if(robotAxisesCount!=jointNum)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====读文件失败: robotAxisesCount!=jointNum";
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5407,robotId);
        return -1;
    }


    QVector<bool> okReturn;
    okReturn.resize(D_PARAMETER_NODE_COUNT);
    for(int i=0;i<robotAxisesCount;i++)
    {
         tmpNode =  findSubNode(inNode, QString("axis%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"ZeroManagement::readFile()====读内部轴参数失败: axis "<<i<<fileName;
            }
            status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5403,robotId,i);
            return -1;
         }

        //节点读取
        readXml( findSubNode(tmpNode, "axisZeroState", okReturn[0]),  axisZeroState[i]  );
        readXml( findSubNode(tmpNode, "absoluteZeroMotorValue", okReturn[1]),  absoluteZeroMotorValue[i]  );
        readXml( findSubNode(tmpNode, "jointMechanicalZeroValue", okReturn[2]),  jointMechanicalZeroValue[i]  );
        readXml( findSubNode(tmpNode, "isAbsoluteEncoder", okReturn[3]),  isAbsoluteEncoder[i]  );
        readXml( findSubNode(tmpNode, "goZeroMethod", okReturn[4]),  zeroReturnParameterList[i].goZeroMethod  );
        readXml( findSubNode(tmpNode, "goZeroSpeed", okReturn[5]),  zeroReturnParameterList[i].goZeroSpeed  );
        readXml( findSubNode(tmpNode, "goZeroAcceleration", okReturn[6]),  zeroReturnParameterList[i].goZeroAcceleration  );
        readXml( findSubNode(tmpNode, "goZeroTorqueLimitPositive", okReturn[7]),  zeroReturnParameterList[i].goZeroTorqueLimitPositive  );
        readXml( findSubNode(tmpNode, "goZeroTorqueLimitNegative", okReturn[8]),  zeroReturnParameterList[i].goZeroTorqueLimitNegative  );
        readXml( findSubNode(tmpNode, "goZeroTimeOutLimit", okReturn[9]),
                zeroReturnParameterList[i].goZeroTimeOutLimit  );
        readXml( findSubNode(tmpNode, "isGoZero", okReturn[10]),
                zeroReturnParameterList[i].isGoZero  );
        readXml( findSubNode(tmpNode, "isNeedGoZeroAfterBoot", okReturn[11]),
                zeroReturnParameterList[i].isNeedGoZeroAfterBoot  );
        readXml( findSubNode(tmpNode, "aiEncoderIndex", okReturn[12]),
                zeroReturnParameterList[i].aiEncoderIndex  );
        readXml( findSubNode(tmpNode, "aiEncoderRatio", okReturn[13]),
                zeroReturnParameterList[i].aiEncoderRatio  );
        readXml( findSubNode(tmpNode, "aiEncoderOffset", okReturn[14]),
                zeroReturnParameterList[i].aiEncoderOffset  );
        readXml( findSubNode(tmpNode, "zeroSensorIndex", okReturn[15]),
                zeroReturnParameterList[i].zeroSensorIndex  );
        readXml( findSubNode(tmpNode, "goZeroSlowSpeed", okReturn[16]),  zeroReturnParameterList[i].goZeroSlowSpeed);
        readXml( findSubNode(tmpNode, "goZeroSequence", okReturn[D_PARAMETER_NODE_COUNT-1]),  zeroReturnParameterList[i].goZeroSequence  );


        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"ZeroManagement::readFile()====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
              addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5404,robotId,i,k);
              return -1;
           }
        }
    }


     closeXml( );

    if (jointNum!=robotAxisesCount)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "ZeroManagement::readFile()===error: 关节数目不对";
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "ZeroManagement", 5405, robotId);
        return -1;
    }

    return 1;
}

int ZeroManagement::writeFile()
{
    bool ok = openXml(fileName, QIODevice::ReadWrite );
    if( !ok )//无法打开机器人数据文件！
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====无法打开机器人零点配置文件！";
        }
        status=ENUM_ZERO_MANAGEMENT_STATE_FILE_NODE_ERROR;
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "ZeroManagement", 5400, robotId);

        return -1;
    }
    int robotAxisesCount=0;

    QDomNode  tmpNode;

//    //读机器人类型
//    int robot;
//    QDomNode  typeNode = findSubNode(m_root, "commonSetting", ok );
//    if( !ok )
//    {
//        if(IF_DEBUG)
//        {
//            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<fileName;
//        }
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5201, robotId);
//        return -1;
//    }

//    readXml( findSubNode(typeNode, "robot", ok), robot  );
//    if( !ok)
//    {
//        if(IF_DEBUG)
//        {
//            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotType";
//        }
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId);
//        return -1;
//    }


    ///读内部轴参数
    QDomNode  inNode = findSubNode(m_root, "robotAxis", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====读文件失败: robotAxis"<<fileName;
        }
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5401,robotId);
        return -1;
    }

    readXml( findSubNode(inNode, "count", ok), robotAxisesCount  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"ZeroManagement::readFile()====读文件失败: robotAxis count"<<robotAxisesCount<<fileName;
        }
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5402,robotId);
    }


    QVector<bool> okReturn;
    okReturn.resize(3);
    JointAttribute joint;
    for(int i=0;i<robotAxisesCount;i++)
    {
         tmpNode =  findSubNode(inNode, QString("axis%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"ZeroManagement::readFile()====读内部轴参数失败: axis "<<i<<fileName;
            }
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5403,robotId,i);
            return -1;
         }

        //节点写数据
        writeXml( findSubNode(tmpNode, "axisZeroState", okReturn[0]),  axisZeroState[i]  );
        writeXml( findSubNode(tmpNode, "absoluteZeroMotorValue", okReturn[1]),  absoluteZeroMotorValue[i]  );
        writeXml( findSubNode(tmpNode, "jointMechanicalZeroValue", okReturn[2]),  jointMechanicalZeroValue[i]  );

        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"ZeroManagement::readFile()====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","ZeroManagement",5404,robotId,i,k);
              return -1;
           }
        }
    }


     closeXml(true);//保存

    if (jointNum!=robotAxisesCount)
    {
        if(IF_DEBUG)
        {
            qDebug()<< "ZeroManagement::readFile()===error: 关节数目不对";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "ZeroManagement", 5405, robotId);
        return -1;
    }

    return 1;

}

int ZeroManagement::checkIncrementEncoder()
{
    for(int i=0;i<isAbsoluteEncoder.size();i++)
    {
        if(false==isAbsoluteEncoder[i])
        {
            axisZeroState[i]=ENUM_AXIS_ZERO_STATE_NO_CALIBRATE ;
        }
    }

    return 1;

}


void ZeroManagement::addMsg(int messageLevel, std::string componentName, std::string messageType
                            , int messageCode, int _robotId, int parameter1, int parameter2)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = _robotId;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 5409://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")无法找到零点配置文件的zeroTaskMode节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5408:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",轴%1回零模式不支持").arg(parameter1+1);
        infomationStr = tmpStr.toStdString();
        break;
    }

    case 5407://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人零点轴数配置与算法轴数配置不一致错误").toStdString();
        infomationStr = str1;
        break;
    }
    case 5406://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人零点配置文件md5校验出错,请重新标定零点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5405://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人零点配置文件轴数不对！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5404:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(_robotId).toStdString() +
               QObject::tr(")机器人零点配置文件  axis").toStdString() +
               QString::number(parameter1+1).toStdString() +
               QObject::tr("缺少参数:").toStdString()+
               QString::number(parameter2+1).toStdString();
       infomationStr = str1;
       break;
    }

    case 5403://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")无法找到零点配置文件的axis节点！").toStdString();
        infomationStr = str1;
        break;
    }

    case 5402://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")无法找到零点配置文件的robotAxis节点！").toStdString();
        infomationStr = str1;
        break;
    }

            case 5401://ENUM_MSG_ERROR
            {
                string str1;
                str1 = QObject::tr("机器人(ID:").toStdString() +
                        QString::number(_robotId).toStdString() +
                        QObject::tr(")无法找到零点配置文件的robotAxis节点！").toStdString();
                infomationStr = str1;
                break;
            }
    case 5400://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")无法打开机器人零点配置文件！").toStdString();
        infomationStr = str1;
        break;
    }



        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
